unit Path;

interface

uses Classes, Math, Mathematic, Vector, Obstacle, GL, GLU;

type
  TPathPoint = class(TVector)
  private
    aRadius: TFloat;
    aForced: Boolean;
    aPoints: TPointsList;
    aID: integer;
  public
    constructor Create;overload;
    constructor Create(paX, paY, paZ: TFloat);overload;
    constructor Create(paX, paY, paZ, paRadius: TFloat; paForced: Boolean);overload;
    constructor CreateCopy(paPathPoint: TPathPoint);
    destructor Destroy;override;
    function SaveToFile(var F: Text): Boolean; override;
    function LoadFromFile(var F: Text): Boolean; override;
    procedure Draw;
    procedure AddPoint(paPoint: TVector);
    property Radius: TFloat read aRadius write aRadius;
    property Forced: Boolean read aForced write aForced;
    property Points: TPointsList read aPoints write aPoints;
    property ID: integer read aID write aID;
  end;

  TPathPointsList = class(TList)
  private
  public
    constructor Create;
    constructor CreateCopy(paPathPointsList: TPathPointsList);
    destructor Destroy;override;
    function SaveToFile(var F: Text): Boolean;
    function LoadFromFile(var F: Text): Boolean;
    procedure Draw;
  end;

  TPath = class(TPathPointsList)
  private
  public
    constructor Create;
    constructor CreateCopy(paPath: TPath);
    destructor Destroy;override;
    procedure Draw;
  end;

  TPath2 = class(TObstaclesList)
  private
    aPathPoints : tObstaclesList;
  public
    constructor Create;
    constructor CreateCopy(paPath: TPath2);
    destructor Destroy;
    procedure Draw(paPathPoints: tObstaclesList);
    procedure Reinitialize;
    property PathPoints: tObstaclesList read aPathPoints write aPathPoints;
  end;

  TDistanceMatrix = array of array of TFloat;
  TPathPointsMatrix = array of array of Integer;
  TAdjacencyMatrix = array of array of Boolean;

  TPathGraph = class
  private
    aPathPoints: TPathPointsList;
    aDistanceMatrix: TDistanceMatrix;
    aPathPointsMatrix: TPathPointsMatrix;
    aAdjacencyMatrix: TAdjacencyMatrix;
  public
    constructor Create(paPathPoints: TPathPointsList);
    constructor CreateCopy(paPathGraph: TPathGraph);
    destructor Destroy;override;
    procedure Initialize;
    procedure Reinitialize;
    procedure Generate(paObstacles: TObstaclesList);
    procedure Compute;
    procedure Draw;
    function GetNearPathPoint(paPosition: TVector) : Integer;
    function GetPath(paPosition, paDestination: TVector) : TPath;
    property DistanceMatrix: TDistanceMatrix read aDistanceMatrix write aDistanceMatrix;
    property PathPointsMatrix: TPathPointsMatrix read aPathPointsMatrix write aPathPointsMatrix;
    property AdjacencyMatrix: TAdjacencyMatrix read aAdjacencyMatrix write aAdjacencyMatrix;

  end;

implementation

constructor TPathPoint.Create;
begin
    inherited Create;
    aRadius := 0.5;
    aForced := false;
    aPoints := TPointsList.Create;
end;

procedure TPathPoint.AddPoint(paPoint: TVector);
begin
  aPoints.Add(paPoint);
end;


constructor TPathPoint.Create(paX, paY, paZ: TFloat);
begin
    inherited Create(paX, paY, paZ);
    aRadius := 0.5;
    aForced := false;
end;

constructor TPathPoint.Create(paX, paY, paZ, paRadius: TFloat; paForced: Boolean);
begin
    inherited Create(paX, paY, paZ);
    aRadius := paRadius;
    aForced := paForced;
end;

constructor TPathPoint.CreateCopy(paPathPoint: TPathPoint);
begin
    inherited CreateCopy(paPathPoint);
    aRadius := paPathPoint.aRadius;
    aForced := paPathPoint.aForced;
end;

function TPathPoint.SaveToFile(var F: Text): Boolean;
begin
    inherited SaveToFile(F);
    FloatToFile(aRadius, F);
    if aForced then IntegerToFile(1, F)
    else IntegerToFile(0, F);
end;

function TPathPoint.LoadFromFile(var F: Text): Boolean;
var I: Integer;
begin
    inherited LoadFromFile(F);
    FloatFromFile(aRadius, F);
    IntegerFromFile(I, F);
    if I = 1 then aForced := true
    else aForced := false;
end;

destructor TPathPoint.Destroy;
begin
    inherited Destroy;
end;

procedure TPathPoint.Draw;
var Quad: PGLUquadric;
begin
    glColor4d(0.0, 0.0, 0.0, 0.0);
    glPointSize(4.0);
    glBegin(GL_POINTS);
    glVertex3dv(PGLDouble(TVector(DataPointer)));
    glEnd;
    Quad := gluNewQuadric;
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix;
    glTranslated(X, Y, Z);
    glColor4d(0.0, 0.0, 0.0, 0.0);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    gluDisk(Quad, aRadius, aRadius, 64, 1);
    glColor4f(0.0, 0.0, 0.0, 1.0);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    gluDisk(Quad, 0.0, aRadius, 64, 1);
    gluDeleteQuadric(Quad);
    glPopMatrix;
end;

constructor TPathPointsList.Create;
begin
    inherited Create;
end;

constructor TPathPointsList.CreateCopy(paPathPointsList: TPathPointsList);
begin
    inherited Create;
end;

destructor TPathPointsList.Destroy;
begin
    inherited Destroy;
end;

function TPathPointsList.SaveToFile(var F: Text): Boolean;
var I: Integer;
begin
    IntegerToFile(Count, F);
    for I := 0 to Count - 1  do begin
        TPathPoint(Items[I]).SaveToFile(F);
    end;
end;

function TPathPointsList.LoadFromFile(var F: Text): Boolean;
var I, C: Integer;
    T: String;
    PathPoint: TPathPoint;
begin
    IntegerFromFile(C, F);
    for I := 0 to C - 1  do begin
        PathPoint :=  TPathPoint.Create;
        PathPoint.LoadFromFile(F);
        Add(PathPoint);
    end;
end;

procedure TPathPointsList.Draw;
var I: Integer;
begin
    for I := 0 to Count - 1  do begin
        TPathPoint(Items[I]).Draw;
    end;
end;

constructor TPath.Create;
begin
    inherited Create;
end;

constructor TPath.CreateCopy(paPath: TPath);
begin
    inherited Create;
end;

destructor TPath.Destroy;
var I: Integer;
begin
    for I := 0 to Count - 1 do begin
        TPathPoint(Items[I]).Free;
    end;
    inherited Destroy;
end;

procedure TPath.Draw;
var I: Integer;
begin
    glColor4d(0.0, 1.0, 0.0, 0.0);
    glPointSize(4.0);
    glBegin(GL_POINTS);
    for I := 0 to Count - 1 do begin
        glVertex3dv(PGLDouble(TVector(Items[I]).DataPointer));
    end;
    glEnd;
    glLineWidth(1.0);
    glBegin(GL_LINE_STRIP);
    for I := 0 to Count - 1 do begin
        glVertex3dv(PGLDouble(TVector(Items[I]).DataPointer));
    end;
    glEnd;
    glLineWidth(1.0);
end;

constructor TPath2.Create;
begin
  PathPoints:= tObstaclesList.Create;
  inherited Create;
end;

constructor TPath2.CreateCopy(paPath: TPath2);
begin
  inherited Create;
end;

destructor TPath2.Destroy;
var i: integer;
begin
  for i := 0 to PathPoints.Count - 1 do
    TObstacle(PathPoints[i]).Free;
  PathPoints.Free;
  inherited Destroy;
end;

procedure TPath2.Draw(paPathPoints: TObstaclesList);
var
  i: Integer;
begin
  for i := 1 to PathPoints.Count - 1 do begin
    glLineWidth(1.0);
    glBegin(GL_LINES);
    glVertex3d(TVector(TPolygonObstacle(paPathPoints[i-1]).Position).X,
               TVector(TPolygonObstacle(paPathPoints[i-1]).Position).Y, 0.0);
    glVertex3d(TVector(TPolygonObstacle(paPathPoints[i]).Position).X,
               TVector(TPolygonObstacle(paPathPoints[i]).Position).Y, 0.0);
    glEnd;
    glLineWidth(1.0);
  end;
end;

procedure TPath2.Reinitialize;
var i: Integer;
begin
  PathPoints.Free;
  PathPoints:= TObstaclesList.Create;
end;

constructor TPathGraph.Create(paPathPoints: TPathPointsList);
begin
    inherited Create;
    aPathPoints := paPathPoints;
    Initialize;
end;

constructor TPathGraph.CreateCopy(paPathGraph: TPathGraph);
begin
    inherited Create;
end;

destructor TPathGraph.Destroy;
var I: Integer;
begin
    inherited Destroy;
end;

procedure TPathGraph.Initialize;
var I, J, K: Integer;
begin
    SetLength(aDistanceMatrix, aPathPoints.Count);
    SetLength(aPathPointsMatrix, aPathPoints.Count);
    SetLength(aAdjacencyMatrix, aPathPoints.Count);
    for I := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
        SetLength(aDistanceMatrix[I], aPathPoints.Count);
        SetLength(aPathPointsMatrix[I], aPathPoints.Count);
        SetLength(aAdjacencyMatrix[I], aPathPoints.Count);
        for J := Low(aDistanceMatrix[I]) to High(aDistanceMatrix[I]) do begin
            if I = J then begin
                aDistanceMatrix[I, J] := MaxFloat;
                aPathPointsMatrix[I, J] := I;
            end
            else begin
                aDistanceMatrix[I, J] := MaxFloat / 2.0;
                aPathPointsMatrix[I, J] := MaxInt;
            end;
            aAdjacencyMatrix[I, J] := false;
        end;
    end;
end;

procedure TPathGraph.Reinitialize;
var I, Count: Integer;
begin
    Count := aPathPoints.Count;
    SetLength(aDistanceMatrix, Count);
    SetLength(aPathPointsMatrix, Count);
    SetLength(aAdjacencyMatrix, Count);
    for I := 0 to Count - 1 do begin
        SetLength(aDistanceMatrix[I], Count);
        SetLength(aPathPointsMatrix[I], Count);
        SetLength(aAdjacencyMatrix[I], Count);
    end;
    for I := 0 to Count - 1 do begin
        aDistanceMatrix[I, Count - 1] := MaxFloat / 2.0;
        aPathPointsMatrix[I, Count - 1] := MaxInt;
        aAdjacencyMatrix[I, Count - 1] := false;
        aDistanceMatrix[Count - 1, I] := MaxFloat / 2.0;
        aPathPointsMatrix[Count - 1, I] := MaxInt;
        aAdjacencyMatrix[Count - 1, I] := false;
    end;
    aPathPointsMatrix[Count - 1, Count - 1] := Count -1;
end;

procedure TPathGraph.Generate(paObstacles: TObstaclesList);
var I, J, K: Integer;
    TestVector: TVector;
    TempNearestEdge: TEdge;
    TempDistance, TempAngle: TFloat;
    Obstacle: TObstacle;
    Intersect: Boolean;
begin
    if (paObstacles <> nil) then begin
        TempNearestEdge[0] := nil;
        TempNearestEdge[1] := nil;
        SetLength(aDistanceMatrix, aPathPoints.Count);
        SetLength(aPathPointsMatrix, aPathPoints.Count);
        SetLength(aAdjacencyMatrix, aPathPoints.Count);
        for I := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
            SetLength(aDistanceMatrix[I], aPathPoints.Count);
            SetLength(aPathPointsMatrix[I], aPathPoints.Count);
            SetLength(aAdjacencyMatrix[I], aPathPoints.Count);
            for J := Low(aDistanceMatrix[I]) to High(aDistanceMatrix[I]) do begin
                if I = J then begin
                    aDistanceMatrix[I, J] := MaxFloat / 2.0;
                    aPathPointsMatrix[I, J] := I;
                    aAdjacencyMatrix[I, J] := false;
                end
                else begin
                    TestVector := TVector.CreateCopy(aPathPoints[J]);
                    TestVector.SubtractSelf(aPathPoints[I]);
                    Intersect := false;
                    for K := 0 to paObstacles.Count - 1 do begin
                        Obstacle := paObstacles[K];
                        if (Obstacle is TPolygonObstacle) and (TPolygonObstacle(Obstacle).GetIntersection(aPathPoints[I], TestVector, TempDistance, TempAngle, TempNearestEdge)) then begin
                           Intersect := true;
                           break;
                        end;
                    end;
                    if Intersect then  begin
                        aDistanceMatrix[I, J] := MaxFloat / 2.0;
                        aPathPointsMatrix[I, J] := MaxInt;
                        aAdjacencyMatrix[I, J] := false;
                    end
                    else begin
                        aDistanceMatrix[I, J] := TestVector.Length;
                        aPathPointsMatrix[I, J] := I;
                        aAdjacencyMatrix[I, J] := true;
                    end;
                    TestVector.Free;
                end;
            end;
        end;
    end;
end;

procedure TPathGraph.Compute;
var I, J, K: Integer;
begin
    for K := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
        for I := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
            for J := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
                if aDistanceMatrix[I, J] > aDistanceMatrix[I, K] + aDistanceMatrix[K, J] then begin
                    aDistanceMatrix[I, J] := aDistanceMatrix[I, K] + aDistanceMatrix[K, J];
                    aPathPointsMatrix[I, J] := aPathPointsMatrix[K, J];
                end;
            end;
        end;
    end;
end;

function TPathGraph.GetNearPathPoint(paPosition: TVector) : Integer;
var I: Integer;
    MinimalDistance, Distance: TFloat;
    NearPathPoint: Integer;
begin
    NearPathPoint := MaxInt;
    if aPathPoints <> nil then begin
        MinimalDistance := MaxFloat;
        for I := 0 to aPathPoints.Count - 1 do begin
            Distance := TPathPoint(aPathPoints[I]).Distance(paPosition);
            if Distance < MinimalDistance then begin
                MinimalDistance := Distance;
                NearPathPoint := I;
            end;
        end;
    end;
    Result := NearPathPoint;
end;

function TPathGraph.GetPath(paPosition, paDestination: TVector) : TPath;
var FindPath: TPath;
    PathPoint: TPathPoint;
    A, B, X: Integer;
begin
    FindPath := TPath.Create;
    if (aDistanceMatrix <> nil) and (aPathPointsMatrix <> nil) then begin
        A := GetNearPathPoint(paPosition);
        B := GetNearPathPoint(paDestination);
        if aPathPointsMatrix[A, B] <> MaxInt then begin
            PathPoint := aPathPoints[A];
            FindPath.Add(TPathPoint.CreateCopy(PathPoint));
            X := B;
            while (aPathPointsMatrix[A, X] <> A)do begin
                PathPoint := aPathPoints[aPathPointsMatrix[A, X]];
                FindPath.Insert(1, TPathPoint.CreateCopy(PathPoint));
                X := aPathPointsMatrix[A, X];
            end;
            if A <> B then begin
                PathPoint := aPathPoints[B];
                FindPath.Add(TPathPoint.CreateCopy(PathPoint));
            end;
            FindPath.Add(TPathPoint.Create(paDestination.X, paDestination.Y, paDestination.Z));
        end;
    end;
    Result := FindPath;
end;

procedure TPathGraph.Draw;
var I, J: Integer;
begin
    glColor4d(0.0, 0.0, 0.0, 0.0);
    for I := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
        for J := Low(aDistanceMatrix) to High(aDistanceMatrix) do begin
            if (I <> J) and aAdjacencyMatrix[I, J] then begin
                glLineWidth(1.0);
                glBegin(GL_LINES);
                glVertex3dv(PGLDouble(TVector(aPathPoints[I]).DataPointer));
                glVertex3dv(PGLDouble(TVector(aPathPoints[J]).DataPointer));
                glEnd;
                glLineWidth(1.0);
            end;
        end;
    end;
end;

end.
